package Behaviors.Arbitrators;
import Behaviors.Schemas.Schema;
import Behaviors.Schemas.Vector2dSchema;
import Robot.*;
import Util.Vector2d;
import javax.swing.*;
import Util.*;
import java.awt.Color;
public class VectorAdditionArbitrator extends Arbitrator{

    
    private boolean isDebugging=false;
    private VectorFrame frame;
    private ControlFrame cframe;
    private boolean showFrame = false;

    public VectorAdditionArbitrator(Schema [] schemas, Position2d actuator, boolean showFrame) {
        super(schemas, (Device)actuator);
        this.showFrame = showFrame;

        if(showFrame)
        {
            frame = new VectorFrame("Arbitro",VectorFrame.CARTESIAN_COORD,10,10);
            frame.setVisible(true);
            cframe = new ControlFrame(this);
            cframe.setVisible(true);
        }
    }

    public void setFrame(JFrame frame)
    {
        this.frame = (VectorFrame)frame;
    }
    public VectorAdditionArbitrator(Schema schema, Position2d actuator, boolean showFrame) {
        super(schema, (Device)actuator);
        
        this.showFrame = showFrame;
        if(showFrame)
        {
            frame = new VectorFrame("Arbitro",VectorFrame.CARTESIAN_COORD,10,10);
            frame.setVisible(true);
        }
    }
    
    public synchronized void step(){
        Schema [] schtmp=this.getSchemas();
        Vector2d [] values=new Vector2d[schtmp.length];
        Vector2d total=new Vector2d(0f,0f,0);
        //This is the sum of magnitudes of all vectors.
        float sumMag = 0;
        //this is the magnitude of the biggest vector
        float maxMag = 0;

        for(int i=0;i<schtmp.length;i++){
            
            values[i]=((Vector2dSchema)(schtmp[i])).Value().scale(schtmp[i].getGain());


            float actualMag = values[i].getMag();
            //sum magnitude of all vectors
            sumMag += actualMag;


            if(maxMag < actualMag)
                    maxMag = actualMag;

            if(isDebugging)
                System.out.println("VECTORADDITIONARBITRATOR:\nVector No. "+Integer.toString(i)+" Mag: "+Float.toString(values[i].getMag())+" theta: "+Double.toString(Math.toDegrees(values[i].getAngle())));

        
        }

                
        for(int i=0;i<values.length;i++){
            total=total.add(values[i]);
            
        }
        if(sumMag == 0  && maxMag == 0)
                maxMag = sumMag = 1;
        total = total.scale(maxMag/sumMag);

        Vector2d[] copyVal = new Vector2d[values.length+1];

        String[] names = new String[values.length+1];
        if(showFrame)
        {
            frame.resetDynElements();

            for(int j=0;j<values.length;j++)
            {
                Color c = Color.GRAY;
                switch(j)
                {
                    case 0:
                    case 4:
                    case 8:
                        c = Color.BLUE;
                    break;
                    case 1:
                    case 5:
                    case 9:
                        c = Color.GREEN;
                    break;
                    case 2:
                    case 6:
                    case 10:
                        c = Color.ORANGE;
                    break;
                    case 3:
                    case 7:
                    case 11:
                        c = Color.DARK_GRAY;
                    break;

                }
                frame.addDynElement(schtmp[j].name, frame.VECTOR_ELEMENT,0,values[j].getX(),values[j].getY(),c);
                names[j] = schtmp[j].name;
            }
            frame.addDynElement("TOTAL", frame.VECTOR_ELEMENT,0,total.getX(),total.getY(),Color.RED);

            names[values.length] = "TOTAL";

            frame.repaint();
        }


        if(isDebugging)
            System.out.println("VECTORADDITIONARBITRATOR:\nFinal Mag: "+Float.toString(total.getMag())+" theta: "+Double.toString(Math.toDegrees(total.getAngle())));

        if(enabled)
            ((Position2d)actuator).goToDir(total.getMag(),total.getAngle());
        //else
            //((Position2d)actuator).setSpeed(0, 0);
        
        
        
        
    
    }
        public void setDebugMode(boolean state)
    {
        this.isDebugging = state;
    
    }
}
